import copy

import cv2
import numpy as np
import math

from slot import *


class SlotDetector:
    def __init__(self, confidence_threshold, binary_thres, min_area):
        self.binary_img_ = None
        self.kConfidenceIndex = 5
        self.confidence_threshold_ = confidence_threshold
        self.binary_thres_ = binary_thres
        self.min_area_ = min_area

        self.new_slots_ = None
        self.opt_new_slots_ = None

        self.warp_img = None
        self.warp_roi = None
        self.warp_roib = None

    def Detect(self, src, rst):
        self.new_slots_ = self.findSlots(src, rst)
        self.opt_new_slots_ = self.optimizeSlots(self.new_slots_, src)
        self.new_slots_ = self.opt_new_slots_
        return self.new_slots_

    def findSlots(self, src, rst):
        if len(rst) == 0:
            cv2.putText(src, "frame undetected", (src.shape[1] // 4, src.shape[0] // 2), cv2.FONT_HERSHEY_SIMPLEX, 3.0,
                        (0, 0, 255), 3)
            return []

        raw_slots = []

        for i in range(len(rst)):
            iter_data = rst[i]

            conf = iter_data[self.kConfidenceIndex]
            if conf < self.confidence_threshold_:
                break

            # Draw circles for the detected points
            pts = []
            skip_flag = False
            for j in range(6):
                pt = (int(iter_data[6 + j * 3]), int(iter_data[7 + j * 3]))
                if pt[0] == 0 and pt[1] == 0:
                    skip_flag = True
                    break
                pts.append(pt)
                # cv2.circle(src, pt, 5, (255, 0, 0), 3)
                # cv2.putText(src, str(j + 1), pt,
                #             cv2.FONT_HERSHEY_SIMPLEX, 3.0, (125, 150, 255), 3)
            if skip_flag:
                continue
            # Draw a rectangle for the slot detection area
            # cv2.rectangle(src, (int(iter_data[0]), int(iter_data[1]), int(iter_data[2]), int(iter_data[3])),
            #               (0, 255, 0), 2)
            # print("Slot Data:", iter_data)  # For debugging

            # TODO slot
            slot = Slot()
            slot.rect = (int(iter_data[0]), int(iter_data[1]), int(iter_data[2]), int(iter_data[3]))
            slot.type = SlotType(iter_data[4])  # SlotType enum

            slot.confidence = conf  # Confidence of the slot

            pts_np = np.array(pts)

            if slot.type == SlotType.FRAME:
                # continue
                # 要去除的下标
                indices_to_remove = [3, 5]
                # 使用列表推导式去除指定下标的元素
                pts_np = [pt for idx, pt in enumerate(pts_np) if idx not in indices_to_remove]
                pts_np = [pts_np[0], pts_np[1], pts_np[3], pts_np[2]]
            # else:
            #     continue

            slot.pts = pts_np  # Original corner points
            # print(f'find {slot.pts}')

            slot.center = ((pts[0][0] + pts[1][0] + pts[2][0] + pts[3][0] + pts[4][0] + pts[5][0]) / 6.,
                           (pts[0][1] + pts[1][1] + pts[2][1] + pts[3][1] + pts[4][1] + pts[5][1]) / 6.)
            # Center of the contour
            raw_slots.append(slot)

        if not raw_slots:
            cv2.putText(src, "frame unselected", (src.shape[1] // 4, src.shape[0] // 2), cv2.FONT_HERSHEY_SIMPLEX, 3.0,
                        (0, 0, 255), 3)
            return []

        return raw_slots

    def optimizeSlots(self, slots, src):
        opt_slots = slots
        self.binary_img_ = self.preprocess_image(src)

        for i, slot in enumerate(opt_slots):
            opt_result = False

            if slot.type == SlotType.LEFT_SIDE or slot.type == SlotType.RIGHT_SIDE:
                opt_result = self.DetectSideROI(src, self.binary_img_, slot)
            elif slot.type == SlotType.FRAME:
                opt_result = self.DetectFrameROIPerspective(src, self.binary_img_, slot)

            if not opt_result:
                opt_slots[i], opt_slots[-1] = opt_slots[-1], opt_slots[i]
                opt_slots.pop()
                i -= 1

        return opt_slots

    def preprocess_image(self, rgb_img):
        """
        Converts the input image to grayscale and applies a binary threshold.
        """
        gray_img = cv2.cvtColor(rgb_img, cv2.COLOR_RGB2GRAY)
        _, binary_img = cv2.threshold(gray_img, self.binary_thres_, 255, cv2.THRESH_BINARY)
        return binary_img

    def get_cross(self, line1_p1, line1_p2, line2_p1, line2_p2):
        """
        Computes the intersection of two lines defined by points (line1_p1, line1_p2) and (line2_p1, line2_p2).
        """
        x0 = ((line2_p1[0] - line2_p2[0]) * (line1_p2[0] * line1_p1[1] - line1_p1[0] * line1_p2[1]) -
              (line1_p1[0] - line1_p2[0]) * (line2_p2[0] * line2_p1[1] - line2_p1[0] * line2_p2[1])) / \
             ((line2_p1[0] - line2_p2[0]) * (line1_p1[1] - line1_p2[1]) -
              (line1_p1[0] - line1_p2[0]) * (line2_p1[1] - line2_p2[1]))
        y0 = ((line2_p1[1] - line2_p2[1]) * (line1_p2[1] * line1_p1[0] - line1_p1[1] * line1_p2[0]) -
              (line1_p1[1] - line1_p2[1]) * (line2_p2[1] * line2_p1[0] - line2_p1[1] * line2_p2[0])) / \
             ((line2_p1[1] - line2_p2[1]) * (line1_p1[0] - line1_p2[0]) -
              (line1_p1[1] - line1_p2[1]) * (line2_p1[0] - line2_p2[0]))

        return x0, y0

    def avoid_out_of_range(self, p1_x, p1_y, p2_x, p2_y, max_width, max_height):
        # print( p1_x, p1_y, p2_x, p2_y, max_width, max_height)
        """
        Adjusts the coordinates (p1, p2) to ensure they are within the image dimensions.
        """
        p1_x = max(0.0, p1_x)
        p1_x = min(float(max_width) - 1.0, p1_x)
        p2_x = max(0.0, p2_x)
        p2_x = min(float(max_width) - 1.0, p2_x)
        p1_y = max(0.0, p1_y)
        p1_y = min(float(max_height) - 1.0, p1_y)
        p2_y = max(0.0, p2_y)
        p2_y = min(float(max_height) - 1.0, p2_y)

        return p1_x, p1_y, p2_x, p2_y

    def avoid_out_of_range_points(self, p1, p2, max_width, max_height):
        """
        Adjusts the points p1, p2 to ensure they are within the image dimensions.
        """
        p1[0] = max(0.0, p1[0])
        p1[0] = min(float(max_width) - 1.0, p1[0])
        p2[0] = max(0.0, p2[0])
        p2[0] = min(float(max_width) - 1.0, p2[0])
        p1[1] = max(0.0, p1[1])
        p1[1] = min(float(max_height) - 1.0, p1[1])
        p2[1] = max(0.0, p2[1])
        p2[1] = min(float(max_height) - 1.0, p2[1])

        return p1, p2

    def recover_origin_point(self, m_inverse, m_point_x, m_point_y):
        """
        Recovers the original point from the inverse transformation matrix.
        """
        m_point = np.array([[m_point_x], [m_point_y], [1.0]])
        o_point = np.dot(m_inverse, m_point)
        return o_point[0][0], o_point[1][0]

    def recover_origin_point_from_point(self, m_inverse, m_point):
        """
        Recovers the original point from the inverse transformation matrix and the point.
        """
        return self.recover_origin_point(m_inverse, m_point[0], m_point[1])

    def recover_origin_point_perspective(self, m_inverse, m_pt):
        """
        Recovers the original point from the perspective transformation matrix.
        """
        # print(m_inverse)
        # print(m_pt)
        m_point = np.array([[m_pt[0]], [m_pt[1]], [1.0]])
        o_point = np.dot(m_inverse, m_point)
        return o_point[0][0] / o_point[2][0], o_point[1][0] / o_point[2][0]

    def get_included_angle(self, pt1, pt2, pt0):
        """
        Calculates the included angle in radians between three points.
        """
        dx1 = pt1[0] - pt0[0]
        dy1 = pt1[1] - pt0[1]
        dx2 = pt2[0] - pt0[0]
        dy2 = pt2[1] - pt0[1]
        angle_line = (dx1 * dx2 + dy1 * dy2) / np.sqrt((dx1 ** 2 + dy1 ** 2) * (dx2 ** 2 + dy2 ** 2) + 1e-10)
        return np.arccos(angle_line)

    def DetectFrameROI(self, src, binary_img, slot):
        r_rect = cv2.minAreaRect(slot.pts)
        # Expand border
        offset = max(r_rect[1][0], r_rect[1][1]) * 0.15 * 2

        r_rect = ((r_rect[0], (r_rect[1][0] + offset, r_rect[1][1] + offset), r_rect[2]))

        rrect_size = r_rect[1]
        center = r_rect[0]  # Center of the minimum bounding rectangle

        # Get rotation matrix and rotate around the center
        M = cv2.getRotationMatrix2D(center, r_rect[2], 1)
        self.warp_img = cv2.warpAffine(src, M, (src.shape[1], src.shape[0]))
        binary_img = self.preprocess_image(self.warp_img)

        # Calculate coordinates for ROI
        sub_p1_x = center[0] - rrect_size[0] / 2
        sub_p1_y = center[1] - rrect_size[1] / 2

        sub_p2_x = sub_p1_x + rrect_size[0]
        sub_p2_y = sub_p1_y + rrect_size[1]

        # Prevent out-of-range errors
        sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y = self.avoid_out_of_range(sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y,
                                                                         self.warp_img.shape[1], self.warp_img.shape[0])

        r_roi_rect = (sub_p1_x, sub_p1_y, sub_p2_x - sub_p1_x, sub_p2_y - sub_p1_y)
        self.warp_roi = self.warp_img[int(sub_p1_y):int(sub_p2_y), int(sub_p1_x):int(sub_p2_x)]

        r_center = (self.warp_roi.shape[1] / 2, self.warp_roi.shape[0] / 2)

        # Inverse transformation matrix
        m_inver = cv2.invertAffineTransform(M)

        # Coordinates of ROI points
        roi_p = [
            (0, 0),
            (0, r_roi_rect[3] - 1),
            (r_roi_rect[2] - 1, 0),
            (r_roi_rect[2] - 1, r_roi_rect[3] - 1)
        ]

        self.warp_roib = binary_img[int(sub_p1_y):int(sub_p2_y), int(sub_p1_x):int(sub_p2_x)]
        slot.roi = self.warp_roi

        for i in range(4):
            light = Light()

            # Subdivide into 4 parts, each containing one light strip
            sub_rect = (roi_p[i][0], roi_p[i][1], r_center[0], r_center[1])
            light.sub_rect = sub_rect

            # Extract binary image for the region
            roi_binary_img = self.warp_roib[int(sub_rect[1]):int(sub_rect[3]), int(sub_rect[0]):int(sub_rect[2])].copy()

            result = self.findLight(roi_binary_img, light)

            if not result:
                continue

            # Restore coordinates back to the ROI's
            light.opt_rect[0] += sub_rect[0]
            light.opt_rect[1] += sub_rect[1]
            light.center += np.array([sub_rect[0], sub_rect[1]])

            light.convex_start += np.array([sub_rect[0], sub_rect[1]])
            light.convex_end += np.array([sub_rect[0], sub_rect[1]])
            light.convex_far += np.array([sub_rect[0], sub_rect[1]])

            # Restore back to the original image's coordinates
            light.convex_start = self.recover_origin_point_from_point(m_inver, light.convex_start + np.array(
                [r_roi_rect[0], r_roi_rect[1]]))
            light.convex_end = self.recover_origin_point_from_point(m_inver, light.convex_end + np.array(
                [r_roi_rect[0], r_roi_rect[1]]))
            light.convex_far = self.recover_origin_point_from_point(m_inver, light.convex_far + np.array(
                [r_roi_rect[0], r_roi_rect[1]]))

            slot.lights.append(light)

        # Restore point order
        for light in slot.lights:
            min_dis = float('inf')
            min_index = -1
            for i in range(len(slot.pts)):
                dis = np.linalg.norm(light.convex_far - slot.pts[i])
                if dis < min_dis:
                    min_dis = dis
                    min_index = i
            light.index = min_index

        # Final points for the slot
        slot.opt_pts = slot.pts
        for light in slot.lights:
            slot.opt_pts[light.index] = light.convex_far

        return True

    def movePointsOutwards(self, p1, p2, d):
        # Calculate direction vector
        dx = p2[0] - p1[0]
        dy = p2[1] - p1[1]

        # Calculate vector length
        length = np.sqrt(dx * dx + dy * dy)
        if length == 0:
            raise ValueError("Points p1 and p2 are the same; cannot determine a line.")

        # Unit direction vector
        unit_dx = dx / length
        unit_dy = dy / length

        # Move points outwards
        p1[0] -= unit_dx * d
        p1[1] -= unit_dy * d

        p2[0] += unit_dx * d
        p2[1] += unit_dy * d

    def get_rectangle_coordinates(self, point1, point2):
        # 确定左上角和右下角的坐标
        x1, y1 = point1
        x2, y2 = point2
        left = min(x1, x2)
        top = min(y1, y2)
        right = max(x1, x2)
        bottom = max(y1, y2)

        # 计算矩形的宽度和高度
        width = right - left
        height = bottom - top

        return left, top, right, bottom

    # 已知两个点的坐标可确定一条直线，现有其他四个点，判断四个点中的哪一个里这条直线最近，且距离指定的一个点最近(p2)
    def get_closest_point(self, p1, p2, points):
        # 已知直线上的两点
        x1, y1 = p1
        x2, y2 = p2

        # 计算直线的斜率和截距
        m = (y2 - y1) / (x2 - x1)
        c = y1 - m * x1

        specified_point = p2

        # 计算每个点到直线的距离
        distances_to_line = np.abs(m * points[:, 0] - points[:, 1] + c) / np.sqrt(m ** 2 + 1)

        # 找到离直线最近的两个点
        nearest_points_indices = np.argsort(distances_to_line)[:2]
        nearest_points = points[nearest_points_indices]

        # 计算离指定点最近的点
        distances_to_specified_point = np.linalg.norm(nearest_points - specified_point, axis=1)
        nearest_point_index = np.argmin(distances_to_specified_point)
        nearest_point = nearest_points[nearest_point_index]

        return nearest_point

    def DetectFrameROIPerspective(self, src, binary_img, slot):
        pts = [copy.deepcopy(arr) for arr in slot.pts]  # Original corner points
        # print(f'2 {pts}')

        # for j in range(4):
        #     cv2.circle(src, pts[j], 5, (255, 0, 0), 3)
        #     cv2.putText(src, str(j + 1), pts[j],
        #                 cv2.FONT_HERSHEY_SIMPLEX, 3.0, (10, 255, 10), 3)

        d1 = np.linalg.norm(pts[0] - pts[3])  # Left top to right bottom distance
        d2 = np.linalg.norm(pts[1] - pts[2])  # Left bottom to right top distance
        max_diagonal = max(d1, d2)  # Max diagonal length

        # Expand border
        offset = max_diagonal * 0.1
        self.movePointsOutwards(pts[0], pts[3], offset)
        self.movePointsOutwards(pts[1], pts[2], offset)

        # print(f'3 {pts}')
        # print(f'3 {slot.pts}')

        max_width = max(np.linalg.norm(pts[0] - pts[2]), np.linalg.norm(pts[1] - pts[3]))
        max_length = max(np.linalg.norm(pts[0] - pts[1]), np.linalg.norm(pts[2] - pts[3]))

        target_vertices = [
            (0, 0),
            (0, max_length - 1),
            (max_width - 1, 0),
            (max_width - 1, max_length - 1),
        ]

        # for j in range(4):
        #     cv2.circle(src, pts[j], 5, (255, 0, 0), 3)
        #     cv2.putText(src, str(j + 1), pts[j],
        #                 cv2.FONT_HERSHEY_SIMPLEX, 3.0, (10, 10, 255), 3)

        M = cv2.getPerspectiveTransform(np.array(pts, dtype=np.float32), np.array(target_vertices, dtype=np.float32))
        self.warp_img = cv2.warpPerspective(src, M, (int(max_width), int(max_length)))
        binary_img = self.preprocess_image(self.warp_img)

        self.warp_roi = self.warp_img
        r_center = (self.warp_roi.shape[1] / 2, self.warp_roi.shape[0] / 2)

        m_inver = np.linalg.inv(M)

        roi_p = [
            (0, 0),
            (0, self.warp_roi.shape[0] - 1),
            (self.warp_roi.shape[1] - 1, 0),
            (self.warp_roi.shape[1] - 1, self.warp_roi.shape[0] - 1)
        ]

        self.warp_roib = binary_img
        slot.roi = self.warp_roi

        for i in range(4):
            light = Light()

            sub_rect = self.get_rectangle_coordinates((roi_p[i][0], roi_p[i][1]), (r_center[0], r_center[1]))
            light.sub_rect = sub_rect

            roi_binary_img = self.warp_roib[int(sub_rect[1]):int(sub_rect[3]), int(sub_rect[0]):int(sub_rect[2])].copy()

            # cv2.imshow(str(i), roi_binary_img)
            # cv2.waitKey(0)

            result = self.findLight(roi_binary_img, light)

            if not result:
                continue

            # print(light.opt_rect)
            # print(sub_rect)
            light.opt_rect = (
                light.opt_rect[0] + sub_rect[0], light.opt_rect[1] + sub_rect[1], light.opt_rect[2], light.opt_rect[3])

            light.center += np.array([sub_rect[0], sub_rect[1]])

            # print(light.convex_start)
            # print(np.array([sub_rect[0], sub_rect[1]]))

            light.convex_start += np.array([sub_rect[0], sub_rect[1]], dtype=light.convex_start.dtype)
            light.convex_end += np.array([sub_rect[0], sub_rect[1]], dtype=light.convex_start.dtype)
            light.convex_far += np.array([sub_rect[0], sub_rect[1]], dtype=light.convex_start.dtype)

            light.convex_start = self.recover_origin_point_perspective(m_inver, light.convex_start)
            light.convex_end = self.recover_origin_point_perspective(m_inver, light.convex_end)
            light.convex_far = self.recover_origin_point_perspective(m_inver, light.convex_far)

            # cv2.circle(self.warp_roi, (int(light.opt_rect[0] + sub_rect[0]),
            #                            int(light.opt_rect[1] + sub_rect[1])), 5, (0, 255, 255), -1)
            # cv2.circle(self.warp_roi, (int(light.opt_rect[0] + sub_rect[0] + light.opt_rect[2]),
            #                            int(light.opt_rect[1] + sub_rect[1] + light.opt_rect[3])),
            #            5, (0, 255, 255), -1)

            light.opt_rect_corners = [
                np.array([light.opt_rect[0], light.opt_rect[1]]),
                np.array([light.opt_rect[0] + light.opt_rect[2], light.opt_rect[1] + light.opt_rect[3]]),
                np.array([light.opt_rect[0], light.opt_rect[1] + light.opt_rect[3]]),
                np.array([light.opt_rect[0] + light.opt_rect[2], light.opt_rect[1]])
            ]

            light.opt_rect_corners[0] = self.recover_origin_point_perspective(m_inver, light.opt_rect_corners[0])
            light.opt_rect_corners[1] = self.recover_origin_point_perspective(m_inver, light.opt_rect_corners[1])
            light.opt_rect_corners[2] = self.recover_origin_point_perspective(m_inver, light.opt_rect_corners[2])
            light.opt_rect_corners[3] = self.recover_origin_point_perspective(m_inver, light.opt_rect_corners[3])

            # cv2.circle(src, (int(light.opt_rect_left_corner[0]),
            #                            int(light.opt_rect_left_corner[1])), 5, (0, 255, 255), -1)
            # cv2.circle(src, (int(light.opt_rect_right_corner[0]),
            #                            int(light.opt_rect_right_corner[1])),
            #            5, (0, 255, 255), -1)

            if light.others_pts1 is not None:
                # print(light.others_pts1)
                for j in range(len(light.others_pts1)):
                    light.others_pts1[j] = (
                        light.others_pts1[j][0] + sub_rect[0], light.others_pts1[j][1] + sub_rect[1])
                    light.others_pts1[j] = self.recover_origin_point_perspective(m_inver, light.others_pts1[j])
                    cv2.circle(src, light.others_pts1[j], 3, (255, 255, 0), 1)
                    cv2.putText(src, str(j), light.others_pts1[j],
                                cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 255), 1)

                for j in range(len(light.others_pts2)):
                    light.others_pts2[j] = (
                        light.others_pts2[j][0] + sub_rect[0], light.others_pts2[j][1] + sub_rect[1])
                    light.others_pts2[j] = self.recover_origin_point_perspective(m_inver, light.others_pts2[j])

                    cv2.circle(src, light.others_pts2[j], 3, (255, 0, 255), 1)
                    cv2.putText(src, str(j), light.others_pts2[j],
                                cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 255), 1)

            slot.lights.append(light)

        other_pts1 = None
        other_pts2 = None

        for light in slot.lights:
            min_dis = float('inf')
            min_index = -1
            # print(f'4 {pts}')
            # print(f'4 {slot.pts}')
            for i in range(len(slot.pts)):
                dis = np.linalg.norm(light.convex_far - slot.pts[i])
                if dis < min_dis:
                    min_dis = dis
                    min_index = i
            light.index = min_index
            if light.others_pts1 is not None and light.others_pts2 is not None:
                other_pts1 = light.others_pts1
                other_pts2 = light.others_pts2

        slot.opt_pts = [copy.deepcopy(arr) for arr in slot.pts]
        for light in slot.lights:
            slot.opt_pts[light.index] = light.convex_far

        if other_pts1 is not None and other_pts2 is not None:
            # print(type(other_pts1))
            # print(type(slot.opt_pts))

            # 在行（轴 0）上拼接两个数组
            result = np.concatenate((other_pts1, other_pts2), axis=0)
            p4 = self.get_closest_point(slot.opt_pts[0], slot.opt_pts[2], result)
            p6 = self.get_closest_point(slot.opt_pts[3], slot.opt_pts[2], result)
            # slot.opt_pts.insert(index, element)
            # slot.opt_pts.insert(index, element)
            slot.opt_pts.append(p4)
            slot.opt_pts.append(p6)
            # print(p4)
            # print(p6)

        # # 移动所有轮廓中的点到新的位置 (x, y)
        # x = 50
        # y = 50
        #
        # for light in slot.lights:
        #     for i in range(len(light.max_area_contour)):
        #         light.max_area_contour[i][0][0] += x
        #         light.max_area_contour[i][0][1] += y
        #
        # # 生成四个示例轮廓（假设这些是您的四个单独轮廓）
        # contour_1 = np.array([[10, 10], [50, 10], [50, 50], [10, 50]])
        # contour_2 = np.array([[70, 10], [110, 10], [110, 50], [70, 50]])
        # contour_3 = np.array([[10, 70], [50, 70], [50, 110], [10, 110]])
        # contour_4 = np.array([[70, 70], [110, 70], [110, 110], [70, 110]])
        #
        # # 合并四个轮廓为一个
        # combined_contour = np.concatenate((contour_1, contour_2, contour_3, contour_4))

        print(len(slot.lights))

        if len(slot.lights) != 4:
            return True


        x_list = []
        y_list = []
        for light in slot.lights:
            for opt_rect_corner in light.opt_rect_corners:
                x_list.append(float(opt_rect_corner[0]))
                y_list.append(float(opt_rect_corner[1]))

        slot_rect_x1 = min(x_list)
        slot_rect_x2 = max(x_list)
        slot_rect_y1 = min(y_list)
        slot_rect_y2 = max(y_list)

        dis1 = math.sqrt((slot.opt_pts[0][0] - slot.opt_pts[3][0])**2 + (slot.opt_pts[0][1] - slot.opt_pts[3][1])**2)
        dis2 = math.sqrt((slot.opt_pts[1][0] - slot.opt_pts[2][0])**2 + (slot.opt_pts[1][1] - slot.opt_pts[2][1])**2)
        dis = (dis1 + dis2) / 2
        offset_opt_rect = dis / 160

        w, h = abs(slot_rect_x2 - slot_rect_x1), abs(slot_rect_y2 - slot_rect_y1)

        opt_x1 = slot_rect_x1 - offset_opt_rect
        opt_x2 = slot_rect_x1 + w + offset_opt_rect
        opt_y1 = slot_rect_y1 - offset_opt_rect
        opt_y2 = slot_rect_y1 + h + offset_opt_rect

        image_height, image_width, _ = src.shape
        opt_x1, opt_y1, opt_x2, opt_y2 = self.avoid_out_of_range(opt_x1, opt_y1, opt_x2, opt_y2, image_width, image_height)

        slot.rect = (opt_x1,
                     opt_y1,
                     abs(opt_x1 - opt_x2),
                     abs(opt_y1 - opt_y2))

        return True

    def DetectSideROI(self, src, binary_img, slot):
        # Get the rotated rectangle for the slot points
        r_rect = cv2.minAreaRect(np.array(slot.pts))
        r_rect = [(slot.rect[0] + slot.rect[2] / 2, slot.rect[1] + slot.rect[3] / 2), (slot.rect[2], slot.rect[3]), 0.]
        # print(r_rect)

        # Compute the extended border length
        offset = max(r_rect[1][0], r_rect[1][1]) * 0.15 * 2.0

        # Extend the bounding box
        r_rect = ((r_rect[0][0], r_rect[0][1]),
                  (r_rect[1][0] + offset, r_rect[1][1] + offset),
                  r_rect[2])

        rrect_size = r_rect[1]  # Width, Height of the rotated rectangle
        center = r_rect[0]  # Center of the rectangle

        # Get the rotation matrix to "straighten" the rectangle
        M = cv2.getRotationMatrix2D(center, r_rect[2], 1)
        self.warp_img = cv2.warpAffine(src, M, (src.shape[1], src.shape[0]))

        binary_img = self.preprocess_image(self.warp_img)  # Assuming this function is defined elsewhere

        # Compute top-left and bottom-right corners of the rotated rectangle
        sub_p1_x = center[0] - rrect_size[0] / 2
        sub_p1_y = center[1] - rrect_size[1] / 2
        sub_p2_x = sub_p1_x + rrect_size[0]
        sub_p2_y = sub_p1_y + rrect_size[1]

        # Prevent going out of bounds
        # print(sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y)
        # print( self.warp_img.shape[1], self.warp_img.shape[0])
        sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y = self.avoid_out_of_range(sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y,
                                                                         self.warp_img.shape[1], self.warp_img.shape[0])

        # print(sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y)
        r_roi_rect = (sub_p1_x, sub_p1_y, sub_p2_x, sub_p2_y)  # ROI rectangle

        self.warp_roi = self.warp_img[int(sub_p1_y):int(sub_p2_y), int(sub_p1_x):int(sub_p2_x)]

        # Get center of the warped ROI
        r_center = (self.warp_roi.shape[1] / 2, self.warp_roi.shape[0] / 2)

        # Invert the affine transformation matrix
        m_inver = cv2.invertAffineTransform(M)

        # Extract binary ROI image
        self.warp_roib = binary_img[int(sub_p1_y):int(sub_p2_y), int(sub_p1_x):int(sub_p2_x)]

        # kernel = np.ones((5, 5), np.uint8)
        # self.warp_roib = cv2.erode(self.warp_roib, kernel, iterations=1)
        # self.warp_roib = cv2.bilateralFilter(self.warp_roib, 9, 75, 75)
        # self.warp_roib = cv2.blur(self.warp_roib, (5, 5))

        offset_point = (r_roi_rect[0], r_roi_rect[1])

        slot.roi = self.warp_roi

        light = Light()
        light.sub_rect = r_roi_rect
        result = self.findLightSide(self.warp_roib, light)  # Assuming this function is defined elsewhere

        if not result:
            print("not result")
            return False

        # Adjust light coordinates to match the ROI
        # light.opt_rect[0] += r_roi_rect[0]
        # light.opt_rect[1] += r_roi_rect[1]
        # light.center = (light.center[0] + r_roi_rect[0], light.center[1] + r_roi_rect[1])

        # light.opt_rect = (light.opt_rect[0] + r_roi_rect[0], light.opt_rect[1] + r_roi_rect[1], light.opt_rect[2], light.opt_rect[3])
        #
        # light.center += np.array([r_roi_rect[0], r_roi_rect[1]])

        # print(light.opt_pts)
        #
        # for i in range(len(light.opt_pts)):
        #     light.opt_pts[i] = (light.opt_pts[i][0] + r_roi_rect[0], light.opt_pts[i][1] + r_roi_rect[1])

        # Recover coordinates to original image
        for i in range(len(light.opt_pts)):
            light.opt_pts[i] = self.recover_origin_point_from_point(m_inver, (
                light.opt_pts[i][0] + offset_point[0], light.opt_pts[i][1] + offset_point[1]))

        # print(light.opt_pts)

        slot.lights.append(light)

        index = []
        for pt in light.opt_pts:
            min_dis = float('inf')
            min_index = -1
            for i in range(len(slot.pts)):
                dis = np.linalg.norm(pt - slot.pts[i])
                if dis < min_dis:
                    min_dis = dis
                    min_index = i
            index.append(min_index)
            # x, y = int(pt[0]), int(pt[1])
            # cv2.circle(src, (x, y), 4, (255, 0, 255), 5)
            # cv2.putText(src, str(0), (x, y), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (255, 0, 255), 5)

        has_num = len(list(set(index)))

        if has_num != 6:
            slot.opt_pts = [copy.deepcopy(arr) for arr in light.opt_pts]
        else:
            slot.opt_pts = [copy.deepcopy(arr) for arr in slot.pts]
            for i, element in enumerate(index):
                slot.opt_pts[element] = light.opt_pts[i].copy()

        slot.rect = light.opt_rect
        # print()

        # off = min(, )
        p1x = light.opt_rect[0] - light.opt_rect[2] / 32
        p1y = light.opt_rect[1] - light.opt_rect[3] / 32
        p2x = light.opt_rect[0] + light.opt_rect[2] + light.opt_rect[2] / 32
        p2y = light.opt_rect[1] + light.opt_rect[3] + light.opt_rect[3] / 32
        p1x, p1y, p2x, p2y = self.avoid_out_of_range(p1x, p1y, p2x, p2y, src.shape[1], src.shape[0])
        w = p2x - p1x
        h = p2y - p1y
        slot.rect = (p1x, p1y, w, h)

        # print(index)
        # slot.rect = (light.opt_rect[0] + r_roi_rect[0], light.opt_rect[1] + r_roi_rect[1], light.opt_rect[2], light.opt_rect[3])

        return True

    def findLight(self, binary_img, light):
        # Find the contours in the binary image
        contours, hierarchy = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)

        # Select the largest contour by area
        max_area = 0
        max_area_index = -1
        for i, contour in enumerate(contours):
            area = cv2.contourArea(contour)
            if area > max_area:
                max_area = area
                max_area_index = i

        light.contour_num = len(contours)
        # print(f'light.contour_num {light.contour_num}')
        light.roi_max_area = max_area

        if light.contour_num >= 3:  # 三个灯条
            contour_area_dict = {}

            for i, contour in enumerate(contours):
                if i == max_area_index:
                    continue
                if cv2.contourArea(contour) < 5:
                    print(1)
                    continue

                contour_area_dict[i] = cv2.contourArea(contour)

            # 将字典转换为元组列表并根据值排序 从大到小排序
            sorted_tuples = sorted(contour_area_dict.items(), key=lambda x: x[1], reverse=True)

            # print(sorted_tuples)

            contour_1 = None
            contour_2 = None

            if len(sorted_tuples) >= 1:
                contour_1 = contours[sorted_tuples[0][0]]
            if len(sorted_tuples) >= 2:
                contour_2 = contours[sorted_tuples[1][0]]

            if contour_1 is not None and contour_2 is not None:
                poly_pts1 = cv2.approxPolyDP(
                    contour_1, cv2.arcLength(contour_1, True) * 0.1, True
                )
                # print("contour_1 poly_pts.size() ", len(poly_pts1))
                light.others_pts1 = np.squeeze(poly_pts1, axis=1)

                poly_pts2 = cv2.approxPolyDP(
                    contour_2, cv2.arcLength(contour_2, True) * 0.1, True
                )
                # print("contour_2 poly_pts.size() ", len(poly_pts2))
                light.others_pts2 = np.squeeze(poly_pts2, axis=1)

        if max_area_index == -1:
            # print("max_area_index == -1")
            return False

        max_area_contour = contours[max_area_index]
        light.max_area_contour = max_area_contour

        # Find the bounding rectangle
        opt_rect = cv2.boundingRect(max_area_contour)

        # Find the minimum area rectangle
        r_rect = cv2.minAreaRect(max_area_contour)

        light.opt_rect = opt_rect
        light.center = r_rect[0]
        # print(f'light.opt_rect {opt_rect}')

        # Calculate aspect ratio (height/width or width/height)
        ratio = max(r_rect[1][0], r_rect[1][1]) / min(r_rect[1][0], r_rect[1][1])
        light.ratio = ratio

        try:
            # Convex hull and convexity defects detection
            hull_index = []
            hull_defect = []

            # Get convex hull and convexity defects
            hull_index = cv2.convexHull(max_area_contour, returnPoints=False)
            hull_defect = cv2.convexityDefects(max_area_contour, hull_index)

            # print(hull_defect)
            # print(hull_defect[0])
            if hull_defect is None:
                return False

            depths = set()
            for i in range(hull_defect.shape[0]):
                s, e, f, d = hull_defect[i, 0]
                depth = d / 256
                if depth <= 0:
                    pass
                    # hull_defect.pop()
                else:
                    depths.add(depth)

            if not depths:
                return False

            max_depth = max(depths)
            for i in range(hull_defect.shape[0]):
                s, e, f, d = hull_defect[i, 0]
                depth = d / 256
                if depth >= max_depth:
                    light.convex_start = max_area_contour[s].flatten()
                    light.convex_end = max_area_contour[e].flatten()
                    light.convex_far = max_area_contour[f].flatten()

        except cv2.error as e:
            print(e)
            return False

        return True

    def findLightSide(self, binary_img, light):
        # Find the contours in the binary image
        contours, hierarchy = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)

        # Select the largest contour by area
        max_area = 0
        max_area_index = -1
        for i, contour in enumerate(contours):
            area = cv2.contourArea(contour)
            tmp_rect = cv2.boundingRect(contour)
            area = tmp_rect[2] * tmp_rect[3]
            if area > max_area:
                max_area = area
                max_area_index = i

        light.contour_num = len(contours)
        light.roi_max_area = max_area

        if max_area_index == -1:
            print('side max_area_index == -1')
            print(f' light.contour_num= {light.contour_num}')
            print(f' light.roi_max_area= {light.roi_max_area}')
            return False

        max_area_contour = contours[max_area_index]
        light.max_area_contour = max_area_contour

        # Find the bounding rectangle
        opt_rect = cv2.boundingRect(max_area_contour)

        # Find the minimum area rectangle
        r_rect = cv2.minAreaRect(max_area_contour)

        light.opt_rect = opt_rect
        light.center = r_rect[0]

        # Calculate aspect ratio (height/width or width/height)
        ratio = max(r_rect[1][0], r_rect[1][1]) / min(r_rect[1][0], r_rect[1][1])
        light.ratio = ratio

        # Polygon fitting (e.g., hexagon detection)
        poly_pts = cv2.approxPolyDP(
            max_area_contour, cv2.arcLength(max_area_contour, True) * 0.009, True
        )
        # print("poly_pts.size() ", len(poly_pts))
        light.opt_pts = np.squeeze(poly_pts, axis=1)
        # print(light.opt_pts)

        return True

    def DrawDebug(self, img):
        for s in self.new_slots_:

            cv2.putText(img, str(s.confidence), (int(s.center[0]), int(s.center[1])), cv2.FONT_HERSHEY_SIMPLEX, 3.0,
                        (125, 150, 255), 3)
            if s.rect is not None:
                cv2.rectangle(img, (int(s.rect[0]), int(s.rect[1])),
                              (int(s.rect[0] + s.rect[2]), int(s.rect[1] + s.rect[3])),
                              (255, 0, 0), 2)
            if s.opt_rect is not None:
                cv2.rectangle(img, (int(s.opt_rect[0]), int(s.opt_rect[1])),
                              (int(s.opt_rect[0] + s.opt_rect[2]), int(s.opt_rect[1] + s.opt_rect[3])),
                              (255, 0, 255), 2)

            # print("original points")
            # Draw original points
            # print(s.pts)
            # print(s.opt_pts)
            for i in range(len(s.pts)):
                x, y = int(s.pts[i][0]), int(s.pts[i][1])
                cv2.circle(img, (x, y), 4, (0, 255, 0), 2)
                cv2.putText(img, str(i), (x, y), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 255, 0), 3)

            # print("optimized points")
            # print(s.opt_pts)
            # Draw optimized points
            for i in range(len(s.opt_pts)):
                # print(s.opt_pts[i])
                # print((int(s.opt_pts[i][0]), int(s.opt_pts[i][1])))
                cv2.circle(img, (int(s.opt_pts[i][0]), int(s.opt_pts[i][1])), 3, (0, 0, 255), 1)
                cv2.putText(img, str(i), (int(s.opt_pts[i][0]), int(s.opt_pts[i][1])), cv2.FONT_HERSHEY_SIMPLEX, 2.0,
                            (0, 0, 255), 3)

            # print("end points")
            # cross = self.get_cross(s.pts[0], s.pts[3], s.pts[1], s.pts[2])
            # cv2.circle(img, (int(cross[0]), int(cross[1])), 5, (0, 0, 255), -1)
            # cv2.circle(img, (int(s.center[0]), int(s.center[1])), 5, (255, 255, 255), 1)

            for light in s.lights:
                cv2.rectangle(s.roi, (int(light.sub_rect[0]), int(light.sub_rect[1])), (int(light.sub_rect[2]), int(light.sub_rect[3])), (0, 255, 0), 2)
                cv2.rectangle(s.roi, (int(light.opt_rect[0]), int(light.opt_rect[1])), (int(light.opt_rect[0] + light.opt_rect[2]), int(light.opt_rect[1] + light.opt_rect[3])), (255, 0, 0), 2)

                cv2.circle(s.roi, (int(light.center[0]), int(light.center[1])), 5, (255, 255, 255), -1)
                cv2.putText(s.roi, "ratio: " + str(light.ratio),
                            (int(light.sub_rect[0]), int(light.sub_rect[1] + light.sub_rect[3] // 4)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 255), 1)
                cv2.putText(s.roi, "m_area: " + str(light.roi_max_area),
                            (int(light.sub_rect[0]), int(light.sub_rect[1] + light.sub_rect[3] // 4 + 30)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 255), 1)
                cv2.putText(s.roi, "c_num: " + str(light.contour_num),
                            (int(light.sub_rect[0]), int(light.sub_rect[1] + light.sub_rect[3] // 4 + 60)),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 255), 1)

                for pt in light.pts:
                    cv2.circle(img, pt, 5, (255, 0, 255), 1)
                # print(light.convex_start)
                # cv2.circle(img, (int(light.convex_start[0]), int(light.convex_start[1])), 4, (0, 255, 0), 2)
                # cv2.circle(img, (int(light.convex_end[0]), int(light.convex_end[1])), 4, (255, 0, 255), 2)
                # cv2.circle(img, (int(light.convex_far[0]), int(light.convex_far[1])), 6, (255, 255, 0), -1)

                # cv2.putText(img, str(light.index), (int(light.convex_far[0]), int(light.convex_far[1])), cv2.FONT_HERSHEY_SIMPLEX, 2.0,
                #             (255, 255, 150), 3)
# SlotType class and other required components need to be defined similarly to how they're handled in C++.
